#ifndef G2OTYPES_H
#define G2OTYPES_H

#include "Thirdparty/g2o/g2o/core/base_vertex.h"
#include "Thirdparty/g2o/g2o/core/base_binary_edge.h"
#include "Thirdparty/g2o/g2o/types/types_sba.h"
#include "Thirdparty/g2o/g2o/core/base_multi_edge.h"
#include "Thirdparty/g2o/g2o/core/base_unary_edge.h"

#include <opencv2/core/core.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>

#include <PoseFrame.h>
#include <KeyPoseFrame.h>

#include "Converter.h"
#include <math.h>

namespace msf
{

class KeyPoseFrame;
class PoseFrame;

typedef Eigen::Matrix<double, 6, 1> Vector6d;
typedef Eigen::Matrix<double, 9, 1> Vector9d;
typedef Eigen::Matrix<double, 12, 1> Vector12d;
typedef Eigen::Matrix<double, 15, 1> Vector15d;
typedef Eigen::Matrix<double, 12, 12> Matrix12d;
typedef Eigen::Matrix<double, 15, 15> Matrix15d;
typedef Eigen::Matrix<double, 9, 9> Matrix9d;

Eigen::Matrix3d ExpSO3(const double x, const double y, const double z);
Eigen::Matrix3d ExpSO3(const Eigen::Vector3d &w);

Eigen::Vector3d LogSO3(const Eigen::Matrix3d &R);

Eigen::Matrix3d InverseRightJacobianSO3(const Eigen::Vector3d &v);
Eigen::Matrix3d RightJacobianSO3(const Eigen::Vector3d &v);
Eigen::Matrix3d RightJacobianSO3(const double x, const double y, const double z);

Eigen::Matrix3d Skew(const Eigen::Vector3d &w);
Eigen::Matrix3d InverseRightJacobianSO3(const double x, const double y, const double z);

template <typename T = double>
Eigen::Matrix<T, 3, 3> NormalizeRotation(const Eigen::Matrix<T, 3, 3> &R)
{
    Eigen::JacobiSVD<Eigen::Matrix<T, 3, 3>> svd(R, Eigen::ComputeFullU | Eigen::ComputeFullV);
    return svd.matrixU() * svd.matrixV().transpose();
}

// 相关节点中使用，存放的是imu与odom的内外参
class ImuOdomPose
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    ImuOdomPose() {}
    ImuOdomPose(KeyPoseFrame *pKPF);
    ImuOdomPose(PoseFrame *pPF);
    ImuOdomPose(Eigen::Matrix3d &_Rwo, Eigen::Vector3d &_two, KeyPoseFrame *pKPF);

    void SetParam(const std::vector<Eigen::Matrix3d> &_Row, const std::vector<Eigen::Vector3d> &_tow, const std::vector<Eigen::Matrix3d> &_Rbo,
                    const std::vector<Eigen::Vector3d> &_tbo);

    void Update(const double *pu);                                                   // update in the imu reference
    void UpdateW(const double *pu);                                                  // update in the world reference
    //Eigen::Vector2d Project(const Eigen::Vector3d &Xw, int cam_idx = 0) const;       // Mono
    //Eigen::Vector3d ProjectStereo(const Eigen::Vector3d &Xw, int cam_idx = 0) const; // Stereo
    //bool isDepthPositive(const Eigen::Vector3d &Xw, int cam_idx = 0) const;

public:
    // For IMU
    Eigen::Matrix3d Rwb;
    Eigen::Vector3d twb;

    std::vector<Eigen::Matrix3d> Row;
    std::vector<Eigen::Vector3d> tow;
    std::vector<Eigen::Matrix3d> Rob, Rbo;
    std::vector<Eigen::Vector3d> tob, tbo;
    //double bf;
    //std::vector<GeometricCamera *> pCamera;

    // For posegraph 4DoF
    /*
     Rwb0是IMU odom位姿的初始旋转矩阵，它是在构造函数中赋值的，表示位姿顶点创建时的旋转状态。
     DR是一个3x3的单位矩阵，它用于存储从初始旋转到当前旋转的变换。
     优化过程中，位姿的旋转增量会左乘到DR上，然后再乘以Rwb0，得到新的Rwb。
    */
    Eigen::Matrix3d Rwb0;
    Eigen::Matrix3d DR;

    int its; // 记录更新次数
};

// Optimizable parameters are IMU pose
// 优化中关于位姿的节点，6自由度
/*
* 6：表示顶点的维度，即位姿的最小表示，包括旋转和平移。
* ImuOdomPose：表示顶点的估计值的类型，即位姿的内部表示，可以是四元数、李代数、欧拉角等。
* read和write：用于从文件中读取或写入顶点的信息。
* setToOriginImpl：用于将顶点的估计值设为初始值。
* oplusImpl：用于根据优化过程中的增量更新顶点的估计值。
*/
class VertexPose : public g2o::BaseVertex<6, ImuOdomPose>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    VertexPose() {}
    VertexPose(KeyPoseFrame *pKPF)
    {
        setEstimate(ImuOdomPose(pKPF));
    }
    VertexPose(PoseFrame *pPF)
    {
        setEstimate(ImuOdomPose(pPF));
    }

    virtual bool read(std::istream &is);
    virtual bool write(std::ostream &os) const;

    // 重置函数,设定被优化变量的原始值
    virtual void setToOriginImpl()
    {
    }

    virtual void oplusImpl(const double *update_)
    {
        _estimate.Update(update_);
        updateCache();
    }
};

/** 
 * @brief 速度节点
 */
class VertexVelocity : public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    VertexVelocity() {}
    VertexVelocity(KeyPoseFrame *pKPF);
    VertexVelocity(PoseFrame *pPF);

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    virtual void setToOriginImpl()
    {
    }

    virtual void oplusImpl(const double *update_)
    {
        Eigen::Vector3d uv;
        uv << update_[0], update_[1], update_[2];
        setEstimate(estimate() + uv);
    }
};

/** 
 * @brief 陀螺仪偏置节点
 */
class VertexGyroBias : public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    VertexGyroBias() {}
    VertexGyroBias(KeyPoseFrame *pKPF);
    VertexGyroBias(PoseFrame *pPF);

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    virtual void setToOriginImpl()
    {
    }

    virtual void oplusImpl(const double *update_)
    {
        Eigen::Vector3d ubg;
        ubg << update_[0], update_[1], update_[2];
        setEstimate(estimate() + ubg);
    }
};

/** 
 * @brief 加速度计偏置节点
 */
class VertexAccBias : public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    VertexAccBias() {}
    VertexAccBias(KeyPoseFrame *pKPF);
    VertexAccBias(PoseFrame *pPF);

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    virtual void setToOriginImpl()
    {
    }

    virtual void oplusImpl(const double *update_)
    {
        Eigen::Vector3d uba;
        uba << update_[0], update_[1], update_[2];
        setEstimate(estimate() + uba);
    }
};

// Gravity direction vertex
// 重力方向
class GDirection
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    GDirection() {}
    GDirection(Eigen::Matrix3d pRwg) : Rwg(pRwg) {}

    void Update(const double *pu)
    {
        // 强行优化不可观的数据，会导致不收敛
        Rwg = Rwg * ExpSO3(pu[0], pu[1], 0.0);
    }

    Eigen::Matrix3d Rwg, Rgw;

    int its;
};

/** 
 * @brief 重力方向节点
 */
class VertexGDir : public g2o::BaseVertex<2, GDirection>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    VertexGDir() {}
    VertexGDir(Eigen::Matrix3d pRwg)
    {
        setEstimate(GDirection(pRwg));
    }

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    virtual void setToOriginImpl()
    {
    }

    virtual void oplusImpl(const double *update_)
    {
        _estimate.Update(update_);
        updateCache();
    }
};

// scale vertex
/** 
 * @brief 尺度节点
 */
class VertexScale : public g2o::BaseVertex<1, double>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    VertexScale()
    {
        setEstimate(1.0);
    }
    VertexScale(double ps)
    {
        setEstimate(ps);
    }

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    virtual void setToOriginImpl()
    {
        setEstimate(1.0);
    }

    virtual void oplusImpl(const double *update_)
    {
        setEstimate(estimate() * exp(*update_));
    }
};

/** 
 * @brief 惯性边（误差为残差）
 */
class EdgeInertial : public g2o::BaseMultiEdge<9, Vector9d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgeInertial(IMU::Preintegrated *pInt);

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    void computeError();
    virtual void linearizeOplus();

    // 关于pose1与2的旋转平移速度，以及之间的偏置的信息矩阵
    Eigen::Matrix<double, 24, 24> GetHessian()
    {
        linearizeOplus();
        Eigen::Matrix<double, 9, 24> J;
        J.block<9, 6>(0, 0) = _jacobianOplus[0];
        J.block<9, 3>(0, 6) = _jacobianOplus[1];
        J.block<9, 3>(0, 9) = _jacobianOplus[2];
        J.block<9, 3>(0, 12) = _jacobianOplus[3];
        J.block<9, 6>(0, 15) = _jacobianOplus[4];
        J.block<9, 3>(0, 21) = _jacobianOplus[5];
        return J.transpose() * information() * J;
    }

    // 没用
    Eigen::Matrix<double, 18, 18> GetHessianNoPose1()
    {
        linearizeOplus();
        Eigen::Matrix<double, 9, 18> J;
        J.block<9, 3>(0, 0) = _jacobianOplus[1];
        J.block<9, 3>(0, 3) = _jacobianOplus[2];
        J.block<9, 3>(0, 6) = _jacobianOplus[3];
        J.block<9, 6>(0, 9) = _jacobianOplus[4];
        J.block<9, 3>(0, 15) = _jacobianOplus[5];
        return J.transpose() * information() * J;
    }

    // 关于pose2的旋转平移信息矩阵
    Eigen::Matrix<double, 9, 9> GetHessian2()
    {
        linearizeOplus();
        Eigen::Matrix<double, 9, 9> J;
        J.block<9, 6>(0, 0) = _jacobianOplus[4];
        J.block<9, 3>(0, 6) = _jacobianOplus[5];
        return J.transpose() * information() * J;
    }

    // 预积分中对应的状态对偏置的雅可比
    /*
    * JRg: 表示IMU预积分误差对陀螺仪偏置的雅可比矩阵，它是一个3x3的矩阵，表示旋转误差对陀螺仪偏置的一阶导数
    * JVg: 表示IMU预积分误差对陀螺仪偏置的雅可比矩阵，它是一个3x3的矩阵，表示速度误差对陀螺仪偏置的一阶导数
    * JPg: 表示IMU预积分误差对陀螺仪偏置的雅可比矩阵，它是一个3x3的矩阵，表示位移误差对陀螺仪偏置的一阶导数
    * JVa: 表示IMU预积分误差对加速度计偏置的雅可比矩阵，它是一个3x3的矩阵，表示速度误差对加速度计偏置的一阶导数
    * JPa: 表示IMU预积分误差对加速度计偏置的雅可比矩阵，它是一个3x3的矩阵，表示位移误差对加速度计偏置的一阶导数
    */
    const Eigen::Matrix3d JRg, JVg, JPg;
    const Eigen::Matrix3d JVa, JPa;

    // for debug
    Eigen::Vector3d dp;
    Eigen::Vector3d dv;
    Eigen::Vector3d eR;
    Eigen::Vector3d eV;
    Eigen::Vector3d eP;
    Eigen::Vector3d DP;
    Eigen::Vector3d DV;
    Eigen::Vector3d ba;
    Eigen::Vector3d bg;

    IMU::Preintegrated *mpInt;  // 预积分
    const double dt;  // 预积分时间
    Eigen::Vector3d g;  // 0, 0, -IMU::GRAVITY_VALUE
};

// Edge inertial whre gravity is included as optimizable variable and it is not supposed to be pointing in -z axis, as well as scale
/** 
 * @brief 初始化惯性边（误差为残差）
 */
class EdgeInertialGS : public g2o::BaseMultiEdge<9, Vector9d>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    // EdgeInertialGS(IMU::Preintegrated* pInt);
    EdgeInertialGS(IMU::Preintegrated *pInt);

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    void computeError();
    virtual void linearizeOplus();

    const Eigen::Matrix3d JRg, JVg, JPg;
    const Eigen::Matrix3d JVa, JPa;
    IMU::Preintegrated *mpInt;
    const double dt;
    Eigen::Vector3d g, gI;

    // for debug
    Eigen::Vector3d dp;
    Eigen::Vector3d dv;
    Eigen::Vector3d eR;
    Eigen::Vector3d eV;
    Eigen::Vector3d eP;
    Eigen::Vector3d DP;
    Eigen::Vector3d DV;
    Eigen::Vector3d ba;
    Eigen::Vector3d bg;

    // 关于pose1与2 的旋转平移速度，以及之间的偏置，还有重力方向与尺度的信息矩阵
    Eigen::Matrix<double, 27, 27> GetHessian()
    {
        linearizeOplus();
        Eigen::Matrix<double, 9, 27> J;
        J.block<9, 6>(0, 0) = _jacobianOplus[0];
        J.block<9, 3>(0, 6) = _jacobianOplus[1];
        J.block<9, 3>(0, 9) = _jacobianOplus[2];
        J.block<9, 3>(0, 12) = _jacobianOplus[3];
        J.block<9, 6>(0, 15) = _jacobianOplus[4];
        J.block<9, 3>(0, 21) = _jacobianOplus[5];
        J.block<9, 2>(0, 24) = _jacobianOplus[6];
        J.block<9, 1>(0, 26) = _jacobianOplus[7];
        return J.transpose() * information() * J;
    }

    // 与上面摆放不同
    Eigen::Matrix<double, 27, 27> GetHessian2()
    {
        linearizeOplus();
        Eigen::Matrix<double, 9, 27> J;
        J.block<9, 3>(0, 0) = _jacobianOplus[2];
        J.block<9, 3>(0, 3) = _jacobianOplus[3];
        J.block<9, 2>(0, 6) = _jacobianOplus[6];
        J.block<9, 1>(0, 8) = _jacobianOplus[7];
        J.block<9, 3>(0, 9) = _jacobianOplus[1];
        J.block<9, 3>(0, 12) = _jacobianOplus[5];
        J.block<9, 6>(0, 15) = _jacobianOplus[0];
        J.block<9, 6>(0, 21) = _jacobianOplus[4];
        return J.transpose() * information() * J;
    }

    // 关于偏置，重力方向与尺度的信息矩阵
    Eigen::Matrix<double, 9, 9> GetHessian3()
    {
        linearizeOplus();
        Eigen::Matrix<double, 9, 9> J;
        J.block<9, 3>(0, 0) = _jacobianOplus[2];
        J.block<9, 3>(0, 3) = _jacobianOplus[3];
        J.block<9, 2>(0, 6) = _jacobianOplus[6];
        J.block<9, 1>(0, 8) = _jacobianOplus[7];
        return J.transpose() * information() * J;
    }

    // 下面的没有用到，其实也是获取状态的信息矩阵
    Eigen::Matrix<double, 1, 1> GetHessianScale()
    {
        linearizeOplus();
        Eigen::Matrix<double, 9, 1> J = _jacobianOplus[7];
        return J.transpose() * information() * J;
    }

    Eigen::Matrix<double, 3, 3> GetHessianBiasGyro()
    {
        linearizeOplus();
        Eigen::Matrix<double, 9, 3> J = _jacobianOplus[2];
        return J.transpose() * information() * J;
    }

    Eigen::Matrix<double, 3, 3> GetHessianBiasAcc()
    {
        linearizeOplus();
        Eigen::Matrix<double, 9, 3> J = _jacobianOplus[3];
        return J.transpose() * information() * J;
    }

    Eigen::Matrix<double, 2, 2> GetHessianGDir()
    {
        linearizeOplus();
        Eigen::Matrix<double, 9, 2> J = _jacobianOplus[6];
        return J.transpose() * information() * J;
    }
};

/** 
 * @brief 陀螺仪偏置的二元边，除了残差及重投影误差外的第三个边，控制偏置变化
 */
class EdgeGyroRW : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, VertexGyroBias, VertexGyroBias>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgeGyroRW() {}

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    void computeError()
    {
        const VertexGyroBias *VG1 = static_cast<const VertexGyroBias *>(_vertices[0]);
        const VertexGyroBias *VG2 = static_cast<const VertexGyroBias *>(_vertices[1]);
        _error = VG2->estimate() - VG1->estimate();
    }

    virtual void linearizeOplus()
    {
        _jacobianOplusXi = -Eigen::Matrix3d::Identity();
        _jacobianOplusXj.setIdentity();
    }

    Eigen::Matrix<double, 6, 6> GetHessian()
    {
        linearizeOplus();
        Eigen::Matrix<double, 3, 6> J;
        J.block<3, 3>(0, 0) = _jacobianOplusXi;
        J.block<3, 3>(0, 3) = _jacobianOplusXj;
        return J.transpose() * information() * J;
    }

    Eigen::Matrix3d GetHessian2()
    {
        linearizeOplus();
        return _jacobianOplusXj.transpose() * information() * _jacobianOplusXj;
    }
};

/** 
 * @brief 加速度计偏置的二元边，除了残差及重投影误差外的第三个边，控制偏置变化
 */
class EdgeAccRW : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, VertexAccBias, VertexAccBias>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgeAccRW() {}

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    void computeError()
    {
        const VertexAccBias *VA1 = static_cast<const VertexAccBias *>(_vertices[0]);
        const VertexAccBias *VA2 = static_cast<const VertexAccBias *>(_vertices[1]);
        _error = VA2->estimate() - VA1->estimate();
    }

    virtual void linearizeOplus()
    {
        _jacobianOplusXi = -Eigen::Matrix3d::Identity();
        _jacobianOplusXj.setIdentity();
    }

    Eigen::Matrix<double, 6, 6> GetHessian()
    {
        linearizeOplus();
        Eigen::Matrix<double, 3, 6> J;
        J.block<3, 3>(0, 0) = _jacobianOplusXi;
        J.block<3, 3>(0, 3) = _jacobianOplusXj;
        return J.transpose() * information() * J;
    }

    Eigen::Matrix3d GetHessian2()
    {
        linearizeOplus();
        return _jacobianOplusXj.transpose() * information() * _jacobianOplusXj;
    }
};

/** 
 * @brief 先验类
 */
class ConstraintPoseImu
{
public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      ConstraintPoseImu(
            const Eigen::Matrix3d &Rwb_, const Eigen::Vector3d &twb_, const Eigen::Vector3d &vwb_,
            const Eigen::Vector3d &bg_, const Eigen::Vector3d &ba_, const Matrix15d &H_)
            : Rwb(Rwb_), twb(twb_), vwb(vwb_), bg(bg_), ba(ba_), H(H_)
      {
            H = (H + H) / 2;  // 对称化
            // 令特征值小的位置变为0
            Eigen::SelfAdjointEigenSolver<Eigen::Matrix<double, 15, 15>> es(H);
            Eigen::Matrix<double, 15, 1> eigs = es.eigenvalues();
            for (int i = 0; i < 15; i++)
                  if (eigs[i] < 1e-12)
                  eigs[i] = 0;
            H = es.eigenvectors() * eigs.asDiagonal() * es.eigenvectors().transpose();
      }

      Eigen::Matrix3d Rwb;
      Eigen::Vector3d twb;
      Eigen::Vector3d vwb;
      Eigen::Vector3d bg;
      Eigen::Vector3d ba;
      Matrix15d H;
};


// Priors for biases
/** 
 * @brief 根据给定值的加速度计先验边，目的是把优化量维持在先验附近
 */
class EdgePriorAcc : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexAccBias>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgePriorAcc(const Eigen::Vector3f &bprior_) : bprior(bprior_.cast<double>()) {}

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    void computeError()
    {
        const VertexAccBias *VA = static_cast<const VertexAccBias *>(_vertices[0]);
        _error = bprior - VA->estimate();
    }
    virtual void linearizeOplus();

    Eigen::Matrix<double, 3, 3> GetHessian()
    {
        linearizeOplus();
        return _jacobianOplusXi.transpose() * information() * _jacobianOplusXi;
    }

    const Eigen::Vector3d bprior;
};

/** 
 * @brief 根据给定值的陀螺仪先验边，目的是把优化量维持在先验附近
 */
class EdgePriorGyro : public g2o::BaseUnaryEdge<3, Eigen::Vector3d, VertexGyroBias>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    EdgePriorGyro(const Eigen::Vector3f &bprior_) : bprior(bprior_.cast<double>()) {}

    virtual bool read(std::istream &is) { return false; }
    virtual bool write(std::ostream &os) const { return false; }

    void computeError()
    {
        const VertexGyroBias *VG = static_cast<const VertexGyroBias *>(_vertices[0]);
        _error = bprior - VG->estimate();
    }
    virtual void linearizeOplus();

    Eigen::Matrix<double, 3, 3> GetHessian()
    {
        linearizeOplus();
        return _jacobianOplusXi.transpose() * information() * _jacobianOplusXi;
    }

    const Eigen::Vector3d bprior;
};

}

#endif // G2OTYPES_H